/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.SpeedController;

/**
 *
    private double m_leftMotorValue = 0.0;
    private double m_rightMotorValue = 0.0;

    public Team997Drive(SpeedController frontLeftMotor, SpeedController rearLeftMotor,
            SpeedController frontRightMotor, SpeedController rearRightMotor,
            Joystick leftStick, Joystick rightStick, double deadband, double motorMax) {
        super(frontLeftMotor, rearLeftMotor, frontRightMotor, rearRightMotor);
        m_leftStick = leftStick;
        m_rightStick = rightStick;
        m_deadband = Math.max(deadband, .03);
        m_motorMax = motorMax;
    }

    public Team997Drive(SpeedController frontLeftMotor, SpeedController rearLeftMotor,
            SpeedController frontRightMotor, SpeedController rearRightMotor,
            Joystick leftStick, Joystick rightStick) {
        super(frontLeftMotor, rearLeftMotor, frontRightMotor, rearRightMotor);
        m_leftStick = leftStick;
        m_rightStick = rightStick;
        m_deadband = 0.05;
        m_motorMax = 1.0;
    }
 */
public class Team997Drive extends RobotDrive {

    private Joystick m_leftStick;
    private Joystick m_rightStick;
    private double m_deadband;
    private double m_motorMax;
    private double m_reverseDeadband = -1.0;
    private double m_curve = -1.0;
    private double m_leftMotorValue = 0.0;
    private double m_rightMotorValue = 0.0;

    public Team997Drive(SpeedController frontLeftMotor, SpeedController rearLeftMotor,
            SpeedController frontRightMotor, SpeedController rearRightMotor,
            Joystick leftStick, Joystick rightStick, double deadband, double motorMax) {
        super(frontLeftMotor, rearLeftMotor, frontRightMotor, rearRightMotor);
        m_leftStick = leftStick;
        m_rightStick = rightStick;
        m_deadband = Math.max(deadband, .03);
        m_motorMax = motorMax;
    }

    public Team997Drive(SpeedController frontLeftMotor, SpeedController rearLeftMotor,
            SpeedController frontRightMotor, SpeedController rearRightMotor,
            Joystick leftStick, Joystick rightStick) {
        super(frontLeftMotor, rearLeftMotor, frontRightMotor, rearRightMotor);
        m_leftStick = leftStick;
        m_rightStick = rightStick;
        m_deadband = 0.10;
        m_motorMax = 1.0;
    }

    public Team997Drive(SpeedController leftMotor, SpeedController rightMotor,
            Joystick leftStick, Joystick rightStick, double deadband, double motorMax) {
        super(leftMotor, rightMotor);
        m_leftStick = leftStick;
        m_rightStick = rightStick;
        m_deadband = Math.max(deadband, .03);
        m_motorMax = motorMax;
    }

    public Team997Drive(SpeedController leftMotor, SpeedController rightMotor,
            Joystick leftStick, Joystick rightStick) {
        super(leftMotor, rightMotor);
        m_leftStick = leftStick;
        m_rightStick = rightStick;
        m_deadband = 0.08;
        m_motorMax = 1.0;
    }

    public void tankDrive(double leftValue, double rightValue) {
        leftValue = limit(leftValue);
        rightValue = limit(rightValue);

        leftValue = driveCurve(leftValue);
        rightValue = driveCurve(rightValue);


        m_leftMotorValue = leftValue;
        m_rightMotorValue = rightValue;
        setLeftRightMotorOutputs(leftValue, rightValue);
    }

    public double driveCurve(double value) {
        double returnValue = 0.0;
        if (value > getDeadband()) {
            returnValue = driveFunction(getReverseDeadband(), value);
        } else if (value < (-getDeadband())) {
            returnValue = driveFunction(-getReverseDeadband(), value);
        }
        return limit(returnValue);
    }

    public double driveFunction(double reverseDeadband, double value) {
        return reverseDeadband + (getMotorMax() - getReverseDeadband()) * (getCurve() * value * value * value + (1 - getCurve()) * value);
    }

    public double getLeftMotorValue() {
        return m_leftMotorValue;
    }

    public double getRightMotorValue() {
        return m_rightMotorValue;
    }

    /**
     * Finds the currently set reverse deadband on the left stick
     * @return currently set deadband
     */
    public double getReverseDeadband() {
        //the variable m_reverseDeadband is set by the function setReverseDeadband, its default value is -1, unless the user has set it above 0
        //A negative value for m_reverseDeadband indicates that the deadband should be read in from the Z Axis (throttle) of the joysticks
        //A positive value for m_reverseDeadBand indicates to use the value stored in memory, regardless of the Z Axis postion.
        if (m_reverseDeadband < 0.0) {
            //Ensures that the deadband is between 0 - 1, rather than -1 - 1
            return .5 * -m_leftStick.getZ() + .5;
        } else {
            return m_reverseDeadband;
        }
    }

    /**
     * a positive makes it so it no longer uses the joystick for the curve, and  negative value returns control to the joystick
     * @param reverseDeadband positive value is fixed reverse deadband
     */
    public void setReverseDeadband(double reverseDeadband) {
        m_reverseDeadband = limit(reverseDeadband);
    }

    /**
     * Finds the currently set curve (determined by the right joystick).
     * Curve determines how much of motion curve is linear or cubic.
     * @return current value of curve
     */
    public double getCurve() {
        //the variable m_curve is set by the function setCurve, its default value is -1, unless the user has set it above 0
        //A negative value for m_curve indicates that the curve should be read in from the Z Axis (throttle) of the joysticks
        //A positive value for m_curve indicates to use the value stored in memory, regardless of the Z Axis postion.
        if (m_curve < 0.0) {
            //Ensures hat the curve is between 0 - 1, rather than -1 - 1
            return .5 * -m_rightStick.getZ() + .5;
        } else {
            return m_curve;
        }
    }

    /**
     * Manually changes the curve. Limits curve to -1 - 1.
     * Values closer to 1 are more cubic, closer to 0 is more linear.
     * Curve determines how much of motion curve is linear or cubic.
     * @param curve : Positive = fixed curve, Negative = Joystick determined
     */
    public void setCurve(double curve) {
        m_curve = limit(curve);
    }

    public double getDeadband() {
        return m_deadband;
    }

    /**
     * Manually changes the deadband. Limits deadband to 0 - 1.
     * Deadband determines how sensitive the joysticks are near 0.
     * @param deadband
     */
    public void setDeadband(double deadband) {
        //The two Math functions combined ensure it can only be set between 0 - 1
        m_deadband = Math.max(Math.min(deadband, 1.0), 0.0);
    }

    public double getMotorMax() {
        return m_motorMax;
    }

    /**
     * Manually changes the maximum motor speed.
     * Motormax also can stretch and flatten the acceleration curve.
     * @param motorMax
     */
    public void setMotorMax(double motorMax) {
        //The two Math functions combined ensure it can only be set between 0 - 1
        m_motorMax = Math.max(Math.min(motorMax, 1.0), 0.0);
    }
}
